#!/usr/bin/env python
# -*- coding: UTF-8 -*-
# 增加卷积竖线卷积次数的参数12.7
import cv2
import numpy as np
import sys
import math



# 引入基本的图像处理模块
import Capture_lane_image

# 侧方所用到的形态学操作矩阵
global kernel_set_cefang_line

# 检测直线所用到的全局参量
# 这里设定检测直线所用到的累加器阈值
global threshold_cefang
# 这个是定义检测直线精度的变量，默认值为1，越大，精度越低
global K_cefang

# 定义侧方停车用到的巡线边界变量
global mid_point_cefang

# 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
global left_boundary_cefang_45, left_flag_cefang_45
global right_boundary_cefang_45, right_flag_cefang_45

# 判断单线还是双线的时候，扫描的额外行数
global Extra_lines_cefang

# 判断边线边界的白色点检测阈值
global White_points_dec_thresholds_cefang

# 用于扫描竖线的标志位
global H_line_detect_flag
# 竖线检测出来的数据参数
global H_line_cefang
# 竖线检测时候的卷积次数全局设定
global H_line_iterations


# 进行参数的初始化
def cefang_line_init():
    # 侧方所用到的形态学操作矩阵
    global kernel_set_cefang_line

    # 这里设定检测直线所用到的累加器阈值
    global threshold_cefang
    # 这个是定义检测直线精度的变量，默认值为1，越大，精度越低
    global K_cefang

    # 定义侧方停车用到的巡线边界变量
    global mid_point_cefang

    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
    global left_boundary_cefang_45, left_flag_cefang_45
    global right_boundary_cefang_45, right_flag_cefang_45

    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines_cefang

    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds_cefang

    # 用于扫描竖线的标志位
    global H_line_detect_flag
    # 竖线检测出来的数据参数
    global H_line_cefang
    # 竖线检测时候的卷积次数全局设定
    global H_line_iterations


    # 生成用于形态学操作的矩阵,这里专门用于进行侧方停车的停车线处理
    kernel_set_cefang_line = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                                       ksize=(3, 3),
                                                       anchor=(-1, -1)
                                                       )

    # 直线检测相关------------------------
    # 这里设定检测直线所用到的累加器阈值
    threshold_cefang = 35
    # 这个是定义检测直线精度的变量，默认值为1，越大，精度越低
    K_cefang = 0.9


    # 侧方巡线相关的参数--------------------
    mid_point_cefang = 320

    # 额外扫描行数设置
    Extra_lines_cefang = 30

    # 判断边线边界的白色点检测阈值设为10，检测到10个点就认为这个是巡线的边界
    White_points_dec_thresholds_cefang = 5

    # 用于扫描竖线的标志位
    H_line_detect_flag = 0

    # 竖线检测出来的数据参数
    H_line_cefang = None

    # 竖线检测时候的卷积次数全局设定
    H_line_iterations = 10

    # 引入的模块相关
    # 进行图像处理模块的初始化
    Capture_lane_image.init_lane_detect()


# 通过端点得到斜率
def get_slope(point1, point2):
    if point1[0] > point2[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_cefang:', angle_blue)
        return angle_blue

    elif point2[0] > point1[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_cefang:', angle_blue)
        return angle_blue

    elif point2[0] == point1[0]:
        # print("slope is zero,angle_cefang_line return None")
        # angle_blue = None
        # return angle_blue

        # 这里就说明是90度了，情况特殊
        angle_blue = 90
        return angle_blue

    else:
        print('glope is wrong!!')
        return None


# 9.26
# 想办法检测并且筛选出纵方向的直线
# 10.4
# 这个函数针对小角度进行了升级
def H_line_detect_filter_give_points(bina_image_H):
    # 用于控制扫描竖线的标志位
    global H_line_detect_flag
    # 竖线检测出来的数据参数
    global H_line_cefang
    # 竖线检测时候的卷积次数全局设定
    global H_line_iterations

    # 第一步
    # 获得图片扫描直线，得到斜率
    if H_line_detect_flag == 0:

        # 对获得的侧方专用二值化图片进行初步的裁剪
        # H_img = original_bina_image_H[210:290, :]
        # 图像统一处理，不进行内部裁剪
        H_img = bina_image_H
        # 先进行相关的腐蚀操作吧
        # ----------------------------------------------------------这里进行腐蚀运算的操作
        # 进行腐蚀运算操作
        H_cefang_erode_line_img = cv2.morphologyEx(src=H_img,
                                                   op=cv2.MORPH_ERODE,
                                                   kernel=kernel_set_cefang_line,
                                                   # 腐蚀默认采用的卷积核大小为3*3，默认腐蚀次数10
                                                   # 这样进行腐蚀之后，目前的最小检测角度为73
                                                   # 如果卷积次数为11，则最小检测角度为82
                                                   iterations=H_line_iterations)

        # 显示腐蚀之后的图片，调试的时候用到的，实际运行的时候，就显示一坨白屏
        cv2.imshow('cefang_HHHH_erode_cal:', H_cefang_erode_line_img)
        # ------------------------------------------------------------------------------

        # 这里使用设定的阈值来检测纵向直线
        H_line_cefang = cv2.HoughLinesP(image=H_cefang_erode_line_img,
                                        rho=1 * K_cefang,
                                        theta=np.pi / 180 * K_cefang,
                                        threshold=threshold_cefang,
                                        minLineLength=20,
                                        maxLineGap=20)

        # 如果成功检测到了直线
        if H_line_cefang is not None:

            # 进入下一步操作，不是进入回调，而是直接进行操作
            H_line_detect_flag = 1
            pass

        # 如果没有检测到直线那就直接返回空了
        else:
            print('None_HHHH_line')
            # 没有检测到直线的情况
            # 就返回一个空值
            # 结束操作了
            points_1_none = (0, 0)
            points_2_none = (0, 0)
            blue_line_none = None
            # 返回这次扫描进程结束的标志位
            finish_flag = 1
            return points_1_none, points_2_none, blue_line_none, None, finish_flag

    # 第二步，根据直线的斜率决定下一步做什么
    if H_line_detect_flag == 1:

        # 设置进行操作的线条的序号，默认为0即可，有多条线可以更改序号
        n = 0
        H_points_1 = (H_line_cefang[n][0][0], H_line_cefang[n][0][1])
        H_points_2 = (H_line_cefang[n][0][2], H_line_cefang[n][0][3])

        # 直接获得第一条检测到的直线的斜率
        H_slope = get_slope(H_points_1, H_points_2)

        # 判断检测的斜率是否为None
        if H_slope is not None:

            # 根据斜率进行检测到的直线筛选
            # 角度需要大于70是一定的
            if abs(H_slope) >= 70:
                print('FFFFFFFFFFFFFFFind_Longitudinal_HHHHHHHHHHHHHHHH_line')

                # 在内部再进行一次角度细分
                # 如果满足倾角较为接近90的的话，那就再进行一次细致的扫描，降低误差
                if abs(H_slope) > 82 and H_line_iterations != 11:
                    print('H_slope is > 82 ,enter next scan')

                    # 将卷积的次数设置为11
                    H_line_iterations = 11
                    # 回到第一步
                    H_line_detect_flag = 0

                    # 这次扫描就返回一个空值
                    points_1_none = (0, 0)
                    points_2_none = (0, 0)
                    blue_line_none = None
                    # 就返回扫描进程尚未结束
                    finish_flag = 0

                    return points_1_none, points_2_none, blue_line_none, None, finish_flag


                # 如果已经再次扫描了,或者扫描的角度小于82，就直接返回对应的数据
                else:
                    # 将默认的卷积的次数重新设置为10
                    H_line_iterations = 10

                    # 由于函数需要多次调用，所以将标志位清零
                    H_line_detect_flag = 0

                    # 返回扫描进程已经结束的标志位
                    finish_flag = 1

                    return H_points_1, H_points_2, H_line_cefang, H_slope, finish_flag

            else:
                print('FFFFFFFFFFFFFFind_line_but_not_H_Longitudinal')
                # 就返回一个空值
                points_1_none = (0, 0)
                points_2_none = (0, 0)
                blue_line_none = None
                # 返回扫描进程已经结束的标志位
                finish_flag = 1

                # 由于函数需要多次调用，所以将标志位清零
                H_line_detect_flag = 0

                return points_1_none, points_2_none, blue_line_none, None, finish_flag




        # 斜率如果为空，就报错
        else:
            print('The slope is None')
            # 就返回一个空值
            points_1_none = (0, 0)
            points_2_none = (0, 0)
            blue_line_none = None

            # 返回扫描进程已经结束的标志位
            finish_flag = 1

            # 由于函数需要多次调用，所以将标志位清零
            H_line_detect_flag = 0
            return points_1_none, points_2_none, blue_line_none, None, finish_flag


# 9.26
# 想办法检测并且筛选出横向的直线
def W_line_detect_filter_give_points(bina_image_W):
    # 对获得的侧方专用二值化图片进行初步的裁剪
    # W_img = original_bina_image_W[160:280, 190:450]
    # 图像同意处理，不进行内部裁剪
    W_img = bina_image_W
    # 先进行相关的腐蚀操作吧
    # ----------------------------------------------------------这里进行腐蚀运算的操作
    # 进行腐蚀运算操作
    W_cefang_erode_line_img = cv2.morphologyEx(src=W_img,
                                               op=cv2.MORPH_ERODE,
                                               kernel=kernel_set_cefang_line,
                                               # 腐蚀的采用卷积核大小为3*3，腐蚀次数为5
                                               iterations=8)
    # 显示腐蚀之后的图片
    cv2.imshow('cefang_WWWW_erode_cal:', W_cefang_erode_line_img)
    # ------------------------------------------------------------------------------

    # 这里使用设定的阈值来检测纵向直线
    W_line_cefang = cv2.HoughLinesP(image=W_cefang_erode_line_img,
                                    rho=1 * K_cefang,
                                    theta=np.pi / 180 * K_cefang,
                                    threshold=threshold_cefang,
                                    minLineLength=20,
                                    maxLineGap=20)

    if W_line_cefang is not None:

        # 设置进行操作的线条的序号，默认为0即可，有多条线可以更改序号
        n = 0
        W_points_1 = (W_line_cefang[n][0][0], W_line_cefang[n][0][1])
        W_points_2 = (W_line_cefang[n][0][2], W_line_cefang[n][0][3])

        # 直接获得第一条检测到的直线的斜率
        W_slope = get_slope(W_points_1, W_points_2)

        # 判断检测的斜率是否为空
        if W_slope is not None:

            # 根据斜率进行检测到的直线筛选
            if abs(W_slope) <= 20:
                print('FFFFFFFFFFFFFFFind_horizontal_WWWWWWWWWWWWWWWW_line')
                return W_points_1, W_points_2, W_line_cefang

            else:
                print('FFFFFFFFFFFFFFind_line_but_not_W_horizontal')
                # 就返回一个空值
                points_1_none = (0, 0)
                points_2_none = (0, 0)
                blue_line_none = None
                return points_1_none, points_2_none, blue_line_none

        # 斜率如果为空，就报错
        else:
            print('The slope is None')
            # 就返回一个空值
            points_1_none = (0, 0)
            points_2_none = (0, 0)
            blue_line_none = None
            return points_1_none, points_2_none, blue_line_none

    else:
        print('None_WWWW_line')
        # 没有检测到直线的情况
        # 就返回一个空值
        points_1_none = (0, 0)
        points_2_none = (0, 0)
        blue_line_none = None
        return points_1_none, points_2_none, blue_line_none


# 2024.9.26
# 在纵向直线上对侧方停车位进行扫描巡线,获得对应边界
# 这个目前还没有写
def cefang_get_boundary(bina_img_boundary):
    # 对获得的侧方专用二值化图片进行初步的裁剪
    # H_boundary_img = original_bina_img_boundary[210:290, :]
    # 图像进行统一处理，不进行裁剪
    H_boundary_img = bina_img_boundary

    # 进行巡线检测用到的参数
    global mid_point_cefang

    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
    global left_boundary_cefang_45, left_flag_cefang_45
    global right_boundary_cefang_45, right_flag_cefang_45

    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines_cefang

    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds_cefang

    # 参数都初始化为0

    left_boundary_cefang_45 = 0
    left_flag_cefang_45 = 0

    right_boundary_cefang_45 = 0
    right_flag_cefang_45 = 0

    # 这里拷贝一份图片，用于扫描处理
    img_detect_line = H_boundary_img

    # 从中间往两边进行扫描
    for i in range(mid_point_cefang):
        # 行的扫描

        # 向左扫描
        # 扫描45行+额外的30行
        if 255 in img_detect_line[45:45 + Extra_lines_cefang, mid_point_cefang - i]:
            left_flag_cefang_45 += 1

        # 向右扫描
        # 扫描45行+额外的30行
        if 255 in img_detect_line[45:45 + Extra_lines_cefang, mid_point_cefang + i]:
            right_flag_cefang_45 += 1

        # ---------------------------------------------------------------------行标志的判断
        # 判断白色点的检测标志是否达到阈值

        # ---------------左边界
        # 445行左边界的判断
        if left_flag_cefang_45 == White_points_dec_thresholds_cefang:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            left_boundary_cefang_45 = mid_point_cefang - i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (left_flag_cefang_45 < White_points_dec_thresholds_cefang) and i == 319:
            left_boundary_cefang_45 = 0

        # ---------------右边界
        # 445行右边界的判断
        if right_flag_cefang_45 == White_points_dec_thresholds_cefang:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            right_boundary_cefang_45 = mid_point_cefang + i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (right_flag_cefang_45 < White_points_dec_thresholds_cefang) and i == 319:
            right_boundary_cefang_45 = 640

    return left_boundary_cefang_45, right_boundary_cefang_45


# 这里定义绘制侧方停车所有直线的参数
def draw_line_all(img, line_draw):
    # 首先获得图像的基础尺寸
    img_WID = img.shape[1]
    img_HIG = img.shape[0]

    if line_draw is not None:
        # 进行直线的绘制
        draw_line = img.copy()
        # for m in range(0, 1):
        # 绘制直线的序号
        for n in range(0, len(line_draw)):
            cv2.line(img=draw_line,
                     pt1=(line_draw[n][0][0], line_draw[n][0][1]),
                     pt2=(line_draw[n][0][2], line_draw[n][0][3]),
                     color=(0, 0, 255),
                     thickness=1,
                     lineType=cv2.LINE_AA,
                     shift=0
                     )
        return draw_line
    else:
        print('No cefang line')
        # 由于更改了摄像头位置，图片也更改了尺寸
        draw_line = np.zeros((img_HIG, img_WID, 3), np.uint8)
        return draw_line


# 绘制第一条直线的函数
def draw_line_first(img, line_draw):
    # 首先获得图像的基础尺寸
    img_WID = img.shape[1]
    img_HIG = img.shape[0]

    if line_draw is not None:
        # 进行直线的绘制
        draw_line = img.copy()
        # for m in range(0, 1):
        # 绘制直线的序号
        # 这里设置绘制检测到的第一条直线
        n = 0
        cv2.line(img=draw_line,
                 pt1=(line_draw[n][0][0], line_draw[n][0][1]),
                 pt2=(line_draw[n][0][2], line_draw[n][0][3]),
                 color=(0, 0, 255),
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )
        return draw_line

    else:
        print('No cefang line')
        # 由于更改了摄像头位置，图片也更改了尺寸
        draw_line = np.zeros((img_HIG, img_WID, 3), np.uint8)
        return draw_line


# 计算出侧方巡线模式，这个函数和基本的巡线模式计算相同
def cefang_line_mode(lef_bound_445, rig_bound_445):

    if abs(rig_bound_445 - lef_bound_445) < 100:
        print('turn_back')
        return 0

    if lef_bound_445 != 0 and rig_bound_445 == 640:
        print('turn_right')
        return 1

    elif lef_bound_445 == 0 and rig_bound_445 != 640:
        print('turn_left')
        return 2

    elif lef_bound_445 != 0 and rig_bound_445 != 640:
        print('both line')
        return 3

    elif lef_bound_445 == 0 and rig_bound_445 == 640:
        print('None line')
        return 4


# 根据竖直直线中线巡线结果判断执行动作组次数的函数，并对显示的二值图进行绘制
def Longitudinal_result_action_judge_num(left_bound_255, right_bound_255, img_for_draw):

    # 首先获得侧方巡线模式
    line_mode = cefang_line_mode(left_bound_255, right_bound_255)

    # 根据模式获得对应的中点
    # 首先进行基本中点的设置
    # 设置用来绘制的两个点
    # 高点
    point_45 = (int((left_bound_255 + right_bound_255) / 2), 10)
    # 低点
    point_mid = (320, 75)

    # 根据巡线模式来判断当前图像的中点位置
    # 根据巡线模式来判断
    if line_mode == 0:
        print('cefang_line_mode_0__need_back')
        point_45 = (320, 10)


    elif line_mode == 1:
        # 进行右移运算
        point_45 = (int((left_bound_255 + left_bound_255 + 470) / 2), 10)


    elif line_mode == 2:
        # 进行左移运算
        point_45 = (int((right_bound_255 - 470 + right_bound_255) / 2), 10)


    elif line_mode == 3:
        # 进行双线矫正运算
        point_45 = (int((left_bound_255 + right_bound_255) / 2), 10)


    elif line_mode == 4:
        print('cefang_line_mode_4_wrong')
        # 扫描不到线直接直行
        point_45 = (320, 10)


    else:
        print('cefang_line_mode_???_wrong')


    # 侧方巡线可视化
    if line_mode != 0:
        cv2.line(img=img_for_draw,
                 pt1=point_45,
                 pt2=point_mid,
                 color=255,
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )

    if line_mode == 0 or line_mode == 4:
        cv2.line(img=img_for_draw,
                 pt1=(160, 70),
                 pt2=(480, 70),
                 color=255,
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )
        return None


    # 接下来获得进行动作组的次数
    # if line_mode == 0 or line_mode == 4:

    # 获得中线距离差距
    cefang_x = point_45[0] - 320

    # 如果x大于0，那么说明车子需要向右矫正
    # 如果x小于0，那么说明车子需要向左纠正
    return cefang_x


    pass


if __name__ == '__main__':
    # 获取到摄像头帧数
    # cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # cv_4.2.1.30_cp38
    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_V4L2)  # cv_ubuntu_cp36
    # 进行参数的初始化
    cefang_line_init()
    # 不断读取摄像头图片
    while True:
        # 读取图片
        ret1, frame1 = cap_camera_wig.read()
        # 如果读取到了图片
        if ret1:
            # 进行畸变的矫正
            # camera_get = Straighten_the_camera(frame1)
            # 采用无畸变摄像头就不用进行矫正
            camera_get = frame1
        # 没有读取到就退出进程
        else:
            print("can't get the pic")
            sys.exit()

        # 获得一张用来画线的基本侧方透视图
        warp_img = Capture_lane_image.cefang_warp_image(camera_get)

        # -------------------------------这里是对图片的切割操作1
        # 纵向图像绘制的切割
        warp_img = warp_img[210:290, :]

        # 横向图像绘制的切割
        # warp_img = warp_img[160:280, 190:450]
        # ------------------------------------------------------

        # 直接获得侧方专用的二值化图片
        cefang_bina_image = Capture_lane_image.cefang_image(camera_get)

        # 这里进行统一的图像裁剪
        cefang_bina_image = cefang_bina_image[210:290, :]
        # 进行相关的纵向直线检测
        points_1, points_2, H_cefang_line, slope, H_Finish_flag = H_line_detect_filter_give_points(cefang_bina_image)
        # 进行相关的纵向边界检测
        # left_bound_cefang_255, right_bound_cefang_255 = cefang_get_boundary(cefang_bina_image)
        # 计算角度差值cefang_x
        # cefang_x_F = Longitudinal_result_action_judge_num(left_bound_cefang_255, right_bound_cefang_255, cefang_bina_image)

        # 进行数据的相关打印
        print('slope: ', slope)
        # print("lef: ", left_bound_cefang_255, " rig: ", right_bound_cefang_255)
        # print('cefang_x: ', cefang_x_F)
        # print("mid: ", (left_bound_cefang_255 + right_bound_cefang_255) / 2)



        # 进行相关的纵向直线检测，这里内部应该有图像裁剪
        # points_1, points_2, W_cefang_line = W_line_detect_filter_give_points(cefang_bina_image)


        # -------------
        # 选择性加入一步进行直线的筛选
        # filter_line = cefang_line_filter(cefang_line)
        filter_line = H_cefang_line
        # filter_line = W_cefang_line
        # -------------

        # 进行图像的绘制
        # 这里选择性绘制  所有/第一条  直线
        line_all_img = draw_line_first(warp_img, filter_line)

        # 进行图像的窗口展示
        # cv2.imshow('original_cefang:', warp_img)

        cv2.imshow('bina_cefang:', cefang_bina_image)

        cv2.imshow('line_img_all:', line_all_img)

        # 等待按键指令
        key_wig = cv2.waitKey(1)

        if (key_wig & 0xFF) == ord('q'):
            print("exit")
            break

    # 释放视频资源
    cap_camera_wig.release()
    # 释放窗口
    cv2.destroyAllWindows()
